package it.gius.pe.algorithm.gjk.shapes;

import org.jbox2d.common.Vec2;

import it.gius.pe.SystemCostants;
import it.gius.pe.Shape.witness.SinglePointWitness;
import it.gius.pe.Shape.witness.TwoPointWitness;
import it.gius.pe.Shape.witness.Witness;
import it.gius.pe.algorithm.gjk.DistanceSolution;
import it.gius.pe.algorithm.gjk.IGjkAlgorithmShape;
import it.gius.pe.algorithm.gjk.IGjkContainedShape;
import it.gius.pe.algorithm.gjk.Simplex;
import it.gius.pe.algorithm.gjk.SimplexSolution;

public class GjkMinkDiffShape implements IGjkAlgorithmShape{

	private IGjkContainedShape shapeA,shapeB;
	//private Vec2 startPointA,startPointB;

	//witnesses for the next call
	private /*int*/ Witness witnessA, witnessB;
	
	private TwoPointWitness twoPwitness;

	private Simplex simplexA,simplexB,simplexZ;
	private Vec2 cSupportA,cSupportB,cSupportZ; 

	//private Simplex simplexATemp,simplexBTemp;

	private Vec2 startingPointA, startingPointB;

	public GjkMinkDiffShape(IGjkContainedShape A,IGjkContainedShape B) {
		this.shapeA = A;
		this.shapeB = B;

		//suggestIndexA = suggestIndexB = null/*0*/;

		witnessA = shapeA.getWitnessInstance().clone();
		witnessB = shapeB.getWitnessInstance().clone();

		twoPwitness = new TwoPointWitness();

		//startPointA = shapeA.getStartingPoint(suggestIndexA);
		//startPointB = shapeB.getStartingPoint(suggestIndexB);

		simplexA = new Simplex(Simplex.LARGER_2D_SIMPLEX);
		simplexB = new Simplex(Simplex.LARGER_2D_SIMPLEX);
		simplexZ = new Simplex(Simplex.LARGER_2D_SIMPLEX);

		cSupportA = new Vec2();
		cSupportB = new Vec2();
		cSupportZ = new Vec2();

		//simplexATemp = new Simplex(Simplex.LARGER_2D_SIMPLEX);
		//simplexBTemp = new Simplex(Simplex.LARGER_2D_SIMPLEX);

		startingPointA = new Vec2();
		startingPointB = new Vec2();

	}


	/*@Override
	public Vec2 getStartingPoint() {

		simplexA.vs[0].set(startPointA);
		simplexA.currentDim = Simplex.IS_0_SIMPLEX;
		simplexB.vs[0].set(startPointB);
		simplexB.currentDim = Simplex.IS_0_SIMPLEX;
		simplexZ.vs[0].set(startPointB.sub(startPointA));
		simplexZ.currentDim = Simplex.IS_0_SIMPLEX;
		return simplexZ.vs[0];
	}*/


	@Override
	public Simplex getStartingSimplex() {

		shapeA.getStartingPoint(witnessA, startingPointA);
		simplexA.vs[0].set(startingPointA);
		simplexA.witnesses[0] = witnessA.clone();
		simplexA.currentDim = Simplex.IS_0_SIMPLEX;

		//Vec2 startPointB = shapeB.getStartingPoint(suggestIndexB);
		shapeB.getStartingPoint(witnessB, startingPointB);
		simplexB.vs[0].set(startingPointB);
		simplexB.witnesses[0] = witnessB.clone();
		simplexB.currentDim = Simplex.IS_0_SIMPLEX;

		simplexZ.vs[0].set(startingPointB.sub(startingPointA));
		simplexZ.currentDim = Simplex.IS_0_SIMPLEX;
		return simplexZ;
	}


	@Override
	public Simplex updateSimplex(SimplexSolution simpSol, Vec2 newSupportPoint,Witness witness) {
		boolean supportAlreadyIn = false;

		twoPwitness = (TwoPointWitness)witness;

		//Witness witnessSupB = shapeB.supportPoint(d,cSupportB);
		//Witness witnessSupA = shapeA.supportPoint(SystemCostants.ORIGIN2D.sub(d),cSupportA);
		if(!cSupportZ.equals(newSupportPoint))
		{
			//System.out.println("!cSupportZ.equals(newSupportPoint)");
			cSupportZ.set(newSupportPoint);
			shapeB.getStartingPoint(twoPwitness.secondWitness, cSupportB);
			shapeA.getStartingPoint(twoPwitness.firstWitness, cSupportA);
		}
		//cSupportZ.set(cSupportB);
		//cSupportZ.subLocal(cSupportA);


		int i,j;

		for(i = 0; i< simpSol.numVerticesUsed; i++)
		{
			for(j=0; !simplexZ.vs[j].equals(simpSol.verticesInSolution[i]);j++);

			simplexA.vs[i].set(simplexA.vs[j]);
			simplexA.witnesses[i] = simplexA.witnesses[j];
			simplexB.vs[i].set(simplexB.vs[j]);
			simplexB.witnesses[i] = simplexB.witnesses[j];

			simplexZ.vs[i].set(simplexB.vs[i].sub(simplexA.vs[i]));

			if(simplexZ.vs[i].equals(cSupportZ))
				supportAlreadyIn = true;
		}

		/*for(i =0; i< simpSol.numVerticesUsed;i++)
		{

			for( j=0; !simplexZ.vs[j].equals(simpSol.verticesInSolution[i]);j++);

			simplexATemp.vs[i].set(simplexA.vs[j]);
			simplexATemp.witnesses[i] = simplexA.witnesses[j];
			simplexBTemp.vs[i].set(simplexB.vs[j]);
			simplexBTemp.witnesses[i] = simplexB.witnesses[j];

		}

		for(i =0; i< simpSol.numVerticesUsed;i++)
		{
			simplexA.vs[i].set(simplexATemp.vs[i]);
			simplexA.witnesses[i] = simplexATemp.witnesses[i];
			simplexB.vs[i].set(simplexBTemp.vs[i]);
			simplexB.witnesses[i] = simplexBTemp.witnesses[i];

			simplexZ.vs[i].set(simplexB.vs[i].sub(simplexA.vs[i]));

			/*****
			//
			//if(shapeA instanceof GjkEllipse && simplexZ.vs[i].sub(cSupportZ).length() < 0.1)
				//System.out.println("simplexZ.vs[i].sub(cSupportZ).length(): "+simplexZ.vs[i].sub(cSupportZ).length());

			if(simplexZ.vs[i].equals(cSupportZ))
				supportAlreadyIn = true;
		}*/


		simplexA.currentDim = simpSol.numVerticesUsed;
		simplexB.currentDim = simpSol.numVerticesUsed;
		simplexZ.currentDim = simpSol.numVerticesUsed;



		//witnessA = simplexA.witnesses[simpSol.numVerticesUsed-1];
		//witnessB = simplexB.witnesses[simpSol.numVerticesUsed-1];

		if(supportAlreadyIn)
			return null;

		if(simplexZ.currentDim < Simplex.LARGER_2D_SIMPLEX)
		{
			simplexA.vs[simpSol.numVerticesUsed].set(cSupportA);
			simplexA.witnesses[simpSol.numVerticesUsed] = twoPwitness.firstWitness.clone();//witnessSupA.clone();
			simplexA.currentDim++;

			simplexB.vs[simpSol.numVerticesUsed].set(cSupportB);
			simplexB.witnesses[simpSol.numVerticesUsed] = twoPwitness.secondWitness.clone();//witnessSupB.clone();
			simplexB.currentDim++;

			simplexZ.vs[simpSol.numVerticesUsed].set(cSupportZ);
			simplexZ.currentDim++;
		}

		return simplexZ;
	}


	/*@Override
	public Vec2 supportPoint(Vec2 d) {

		Vec2 supA = shapeA.supportPoint(d);
		Vec2 supB = shapeB.supportPoint( SystemCostants.ORIGIN2D.sub(d));

		return supB.sub(supA);
	}*/

	@Override
	public Witness supportPoint(Vec2 d, Vec2 supportPoint) {

		twoPwitness.secondWitness = (SinglePointWitness)shapeB.supportPoint(d,cSupportB);
		twoPwitness.firstWitness = (SinglePointWitness)shapeA.supportPoint(SystemCostants.ORIGIN2D.sub(d),cSupportA);

		supportPoint.set(cSupportB);
		supportPoint.subLocal(cSupportA);

		cSupportZ.set(supportPoint);

		return twoPwitness;
	}

	@Override
	public void fillSolution(Vec2 pointSol, Vec2 q, SimplexSolution simpSol, DistanceSolution sol) {

		witnessA = getPointSolutionAndUpdateWitness(simplexA, simplexZ, simpSol, sol.p1);
		witnessB = getPointSolutionAndUpdateWitness(simplexB, simplexZ, simpSol, sol.p2);

		sol.lastSimplex = simplexZ;
		//sol.indexPoly1PointInSolution = witnessA;
		//sol.indexPoly2PointInSolution = witnessB;

		if(simpSol.numVerticesUsed == Simplex.LARGER_2D_SIMPLEX)
			sol.distance = 0;

	}



	private Witness getPointSolutionAndUpdateWitness(Simplex simplexAB, Simplex simplexZ, SimplexSolution npS, Vec2 pABSol)
	{
		pABSol.setZero();
		int i,j=0;
		for(i=0; i<npS.numVerticesUsed;i++)
		{

			for( j=0; !simplexZ.vs[j].equals(npS.verticesInSolution[i]);j++);

			pABSol.addLocal(simplexAB.vs[j].mul(npS.coefficients[i]));

		}
		//System.out.println("j: " +j +", numVerticesUsed: " + npS.numVerticesUsed);
		return simplexAB.witnesses[j];

	}


	@Override
	public int getDim() {
		return shapeA.getDim() + shapeB.getDim();
	}

}
